/**
  ******************************************************************************
  * @file	uArmAPI.cpp
  * @author	David.Long	
  * @email	xiaokun.long@ufactory.cc
  * @date	2016-11-28
  ******************************************************************************
  */

#include "uArmAPI.h" 

uArmButton buttonMenu;
uArmButton buttonPlay;

#ifdef MKII
    uArmLed ledRed;
#elif defined(SWIFT)
    uArmLed ledRed;
    uArmLed ledGreen;
    uArmLed ledBlue;

    uArmButton buttonState;
#endif

#ifdef DEBUG 
#define STEP_MAX	30	// no enough ram for debug. so reduce the size to enable debug
#else
#define STEP_MAX	60
#endif

#define INTERP_EASE_INOUT_CUBIC 0  // original cubic ease in/out
#define INTERP_LINEAR           1
#define INTERP_EASE_INOUT       2  // quadratic easing methods
#define INTERP_EASE_IN          3
#define INTERP_EASE_OUT         4

#define STEP_MAX_TIME				20	// ms

#define EXTERNAL_EEPROM_SYS_ADDRESS 0xA2
#define EXTERNAL_EEPROM_USER_ADDRESS  0xA0

#define PUMP_GRABBING_CURRENT 	70    

static int mCurStep;
static int mCurIndex;
static int mTotalSteps;
static unsigned int mTimePerStep;
static unsigned long mStartTime;
static double mPathX[STEP_MAX];
static double mPathY[STEP_MAX];
static double mPathZ[STEP_MAX];

#define MAC_LEN	12
#define MAC_FLAG_SET	0XCE
static unsigned char mMacStr[MAC_LEN+1] = {0};
static unsigned char mHWSubVersion;

static unsigned char _moveTo(double x, double y, double z, double speed);
static void _sort(unsigned int array[], unsigned int len);
static void _controllerRun();
static unsigned char _moveToAngle(uint8_t servoNumber, double angle, double speed);
static void _controllerAngleRun();

void initHardware()
{

	pinMode(BTN_D4, INPUT_PULLUP);        //special mode for calibration
	pinMode(BUZZER, OUTPUT);
	pinMode(LIMIT_SW, INPUT_PULLUP);
	pinMode(BTN_D7, INPUT_PULLUP);
	pinMode(PUMP_EN, OUTPUT);
	pinMode(GRIPPER, OUTPUT);

	#ifdef MKII
	pinMode(SYS_LED, OUTPUT);
	pinMode(POWER_DETECT, INPUT_PULLUP);

	digitalWrite(PUMP_EN, HIGH);//keep the pump off
	#elif defined(METAL) 
	pinMode(VALVE_EN, OUTPUT);
	#elif defined(SWIFT)
	pinMode(BTN_STATE, INPUT_PULLUP);   
	pinMode(VALVE_EN, OUTPUT);
	pinMode(BT_DETECT_PIN, INPUT);
	// pinMode(LED_R, OUTPUT);
	// pinMode(LED_G, OUTPUT);
	// pinMode(LED_B, OUTPUT);	
	pinMode(HW_PIN_0, INPUT_PULLUP);   
	pinMode(HW_PIN_1, INPUT_PULLUP);   
	pinMode(HW_PIN_2, INPUT_PULLUP);   
	pinMode(HW_PIN_3, INPUT_PULLUP);   

	#endif


	buttonMenu.setPin(BTN_D4);
	buttonPlay.setPin(BTN_D7);

	buzzer.setPin(BUZZER);

	#ifdef MKII
	ledRed.setPin(SYS_LED);
	#elif defined(SWIFT)
	ledRed.setPin(LED_R);
	ledGreen.setPin(LED_G);
	ledBlue.setPin(LED_B);

	buttonState.setPin(BTN_STATE);
	#endif


    

}

void clearMacFlag()
{
	setE2PROMData(EEPROM_EXTERN_SYSTEM, MAC_FLAG_ADDRESS, DATA_TYPE_BYTE, 0);
}

void getVersions()
{
#ifdef SWIFT
	// get BT MAC
	// if not set mac flag, get mac from bt
	delay(10);
	unsigned char macFlag = getE2PROMData(EEPROM_EXTERN_SYSTEM, MAC_FLAG_ADDRESS, DATA_TYPE_BYTE);
	debugPrint("mac flag =%d\r\n", macFlag);
	// if flag set then read it from EEPROM
	if (macFlag == MAC_FLAG_SET)
	{
		delay(10);
		iic_readbuf(mMacStr, EXTERNAL_EEPROM_SYS_ADDRESS, MAC_ADDRESS, MAC_LEN);// write data
		mMacStr[MAC_LEN] = '\0';
		debugPrint("mac=%s\r\n", mMacStr);
	}
	else
	{
		unsigned char readBuf[128];
		int i = 0;
		// get from BT
		Serial2.write("AT+ADDR?");
		delay(100);

		// set firmware flag
		Serial2.write("AT+UUID0xFFE2");
		//Serial2.write(BT_FLAG);

		// wait for reply
		delay(1000);

		// get data
		while (Serial2.available())
		{
			readBuf[i++] = Serial2.read();
		}

		readBuf[i] = '\0';
		debugPrint("%s\r\n", readBuf);

		// expect data: OK+ADDR:00FF00FF00FF
		// parse datas
		char *s = strstr(readBuf, "OK+ADDR:");

		if (s != NULL)
		{
			//found
			strcpy(mMacStr, s+8);
			debugPrint("mac=%s\r\n", mMacStr);

			delay(10);
			// store in EEPROM
			iic_writebuf(mMacStr, EXTERNAL_EEPROM_SYS_ADDRESS, MAC_ADDRESS, MAC_LEN);

			delay(10);
			// set mac flag
			setE2PROMData(EEPROM_EXTERN_SYSTEM, MAC_FLAG_ADDRESS, DATA_TYPE_BYTE, MAC_FLAG_SET);
		}

	}

	mHWSubVersion = 0;
	unsigned char bit = 1;
	// get hw version
	if (digitalRead(HW_PIN_0))
	{
		mHWSubVersion |= bit;
	}

	bit <<= 1;
	if (digitalRead(HW_PIN_1))
	{
		mHWSubVersion |= bit;
	}

	bit <<= 1;
	if (digitalRead(HW_PIN_2))
	{
		mHWSubVersion |= bit;
	}

	bit <<= 1;
	if (digitalRead(HW_PIN_3))
	{
		mHWSubVersion |= bit;
	}

	debugPrint("subversion:%d\r\n", mHWSubVersion);
#endif
}

/*!
   \brief init components
 */
void uArmInit()
{
	Wire.begin();
	initHardware();

	reportServiceInit();
	getVersions();

	controller.init();

	mCurStep = -1;
	mTotalSteps = -1; 	

#ifdef SWIFT
	ledRed.off();
	ledBlue.off();
	ledGreen.off();

	pumpOff();
	gripperRelease();
#endif
}

#ifdef SWIFT
/*!
   \brief get mac of bt which as unique code of HW
 */
unsigned char* getMac()
{
	return mMacStr;
}

/*!
   \brief get hardware subvervion
 */
unsigned char getHWSubversion()
{
	return mHWSubVersion;
}
#endif

/*!
   \brief move to pos(x, y, z)
   \param x, y, z in mm
   \param speed: 
   			[0]: move to destination directly
   			[1~99]: change the dutycycle of servo (1~99%)
   			[100~1000]: mm/min, will do interpolation to control the speed and block process util move done
   \return IN_RANGE if everything is OK
   \return OUT_OF_RANGE_NO_SOLUTION if cannot reach
   \return OUT_OF_RANGE will move to the closest pos
   \return NO_NEED_TO_MOVE if it is already there
   \return ERR_NO_POWER_CONNECTED if no power plug in
 */
unsigned char moveTo(double x, double y, double z, double speed)
{
	unsigned char result = IN_RANGE;

	debugPrint("moveTo: x=%f, y=%f, z=%f, speed=%f\r\n", x, y, z, speed);

#if defined( MKII	) || defined(SWIFT)
	if (!isPowerPlugIn)
	{
		return ERR_NO_POWER_CONNECTED;
	}
#endif

	// when speed less than 100 mm/min, move to destination directly
	if (speed < 0)
	{
		return OUT_OF_RANGE_NO_SOLUTION;
	}
	else if (speed < 100)
	{
		unsigned char dutyCycle = map(speed, 0, 99, 0,  255);   
		
		controller.setServoSpeed(dutyCycle);

		double angleB, angleL, angleR;
		result = controller.xyzToAngle(x, y, z, angleB, angleL, angleR);
		if (result != OUT_OF_RANGE_NO_SOLUTION)
		{
			controller.writeServoAngle(angleB, angleL, angleR);
		}

		return result;		
	}
	else
	{
		controller.setServoSpeed(255);
		result = _moveTo(x, y, z, speed);

		if(result != OUT_OF_RANGE_NO_SOLUTION)
		{
			_controllerRun();
			
		}

		return result;
	}
}

/*!
   \brief move to pos(x, y, z) according to current pos
   \param x, y, z (mm)
   \param speed (mm/min)
   \return IN_RANGE if everything is OK
   \return OUT_OF_RANGE_NO_SOLUTION if cannot reach
   \return OUT_OF_RANGE will move to the closest pos
   \return NO_NEED_TO_MOVE if it is already there
   \return ERR_NO_POWER_CONNECTED if no power plug in
 */
unsigned char relativeMove(double x, double y, double z, double speed)
{
	double x1, y1, z1;
	// get cur xyz
	controller.getCurrentXYZ(x1, y1, z1);

	x1 += x;
	y1 += y;
	z1 += z;

	return moveTo(x1, y1, z1, speed);	
}

/*!
   \brief move to pos of polor coordinates(s, r, h)
   \param s: stretch(mm)
   \param r: angle (0~180)
   \param h: height(mm)
   \param speed (mm/min)
   \return IN_RANGE if everything is OK
   \return OUT_OF_RANGE_NO_SOLUTION if cannot reach
   \return OUT_OF_RANGE will move to the closest pos
   \return NO_NEED_TO_MOVE if it is already there
   \return ERR_NO_POWER_CONNECTED if no power plug in
 */
unsigned char moveToPol(double s, double r, double h, double speed)
{
	double x, y, z;

	polToXYZ(s, r, h, x, y, z);
	return moveTo(x, y, z, speed);		
}


/*!
   \brief move to pos of polor coordinates(s, r, h) according to current pos
   \param s: stretch(mm)
   \param r: angle (0~180)
   \param h: height(mm)
   \param speed (mm/min)
   \return IN_RANGE if everything is OK
   \return OUT_OF_RANGE_NO_SOLUTION if cannot reach
   \return OUT_OF_RANGE will move to the closest pos
   \return NO_NEED_TO_MOVE if it is already there
   \return ERR_NO_POWER_CONNECTED if no power plug in
 */
unsigned char relativeMovePol(double s, double r, double h, double speed)
{
	double cur_s, cur_r, cur_h;
	// get current pos of polor coordinates
	getCurrentPosPol(cur_s, cur_r, cur_h);

	// add offset 
	s += cur_s;
	r += cur_r;
	h += cur_h;

	moveToPol(s, r, h, speed);


}

/*!
   \brief attach servo(0~3)
   \param servoNumber: SERVO_ROT_NUM, SERVO_LEFT_NUM, SERVO_RIGHT_NUM, SERVO_HAND_ROT_NUM
   \return true or false
 */
bool attachServo(unsigned char servoNumber)
{
	if (servoNumber < SERVO_COUNT)
	{
		controller.attachServo(servoNumber);
		return true;
	}

	return false;
}

/*!
   \brief is servo attached(0~3)
   \param servoNumber: SERVO_ROT_NUM, SERVO_LEFT_NUM, SERVO_RIGHT_NUM, SERVO_HAND_ROT_NUM
   \return true or false
 */
bool isServoAttached(unsigned char servoNumber)
{
	if (servoNumber >= SERVO_COUNT)
		return false;

	return controller.servoAttached(servoNumber);
}

/*!
   \brief detach servo(0~3)
   \param servoNumber: SERVO_ROT_NUM, SERVO_LEFT_NUM, SERVO_RIGHT_NUM, SERVO_HAND_ROT_NUM
   \return true or false
 */
bool detachServo(unsigned char servoNumber)
{
	if (servoNumber < SERVO_COUNT)
	{
		controller.detachServo(servoNumber);
		return true;
	}

	return false;	
}

/*!
   \brief set servo angle
   \param servoNumber(0~3)
   \param angle (0~180)
   \param speed (deg/min)   
   \return OK if everything is OK
   \return ERR_SERVO_INDEX_EXCEED_LIMIT if servoNumber not in range(0~3)
   \return ERR_ANGLE_OUT_OF_RANGE if angle not in range(0~180)
 */
unsigned char setServoAngle(unsigned char servoNumber, double angle, double speed)
{
	if (servoNumber >= SERVO_COUNT)
		return ERR_SERVO_INDEX_EXCEED_LIMIT;

	unsigned char result = IN_RANGE;

	result = controller.validateAngle(servoNumber, angle);

	if(result == OUT_OF_RANGE_NO_SOLUTION)
	{
		return ERR_ANGLE_OUT_OF_RANGE;
	}

	if (speed == 0)
	{
		controller.writeServoAngle(servoNumber, angle);
	}
	else
	{
		
		result = _moveToAngle(servoNumber, angle, speed);

		if(result != OUT_OF_RANGE_NO_SOLUTION)
		{
			_controllerAngleRun();
			return OK;
		}	


		return result;

		
	}

	return OK;
}

/*!
   \brief get servo angle
   \param servoNumber(0~3)
   \return value of angle
   \return -1 if servoNumber not in range(0~3)
 */
double getServoAngle(unsigned char servoNumber)
{
	if (servoNumber >= SERVO_COUNT)
		return -1;	

	return controller.readServoAngle(servoNumber);
}


/*!
   \brief gripper work
 */
void gripperCatch()
{
	digitalWrite(GRIPPER, LOW); 
}

/*!
   \brief gripper stop
 */
void gripperRelease()
{
 	digitalWrite(GRIPPER, HIGH); 
}

/*!
   \brief get gripper status
   \return STOP if gripper is not working
   \return WORKING if gripper is working but not catched sth
   \return GRABBING if gripper got sth   
 */
unsigned char getGripperStatus()
{
#ifdef MKII
	//Serial.println(getAnalogData(GRIPPER_FEEDBACK));
	if (digitalRead(GRIPPER) == HIGH)
	{
		return STOP;//NOT WORKING
	}
	else
	{

		if (getAnalogPinValue(GRIPPER_FEEDBACK) > 600)
		{
			return WORKING;
		}
		else
		{
			return GRABBING;
		}
	}    
#elif defined(METAL)    
	if(digitalRead(GRIPPER) == HIGH)
	{
		return STOP;
	}
	else
	{
		if(getAnalogPinValue(GRIPPER_FEEDBACK) > 600)
		{
		    return WORKING;
		}
		else
		{
		    return GRABBING;
		}
	}
#elif defined(SWIFT)
	if (digitalRead(GRIPPER) == HIGH)
	{
		return STOP;//NOT WORKING
	}
	else
	{

		if (getAnalogPinValue(GRIPPER_FEEDBACK) > 600)
		{
			return WORKING;
		}
		else
		{
			return GRABBING;
		}
	}   	
#endif
}

/*!
   \brief get pump status
   \return STOP if pump is not working
   \return WORKING if pump is working but not catched sth
   \return GRABBING if pump got sth   
 */
unsigned char getPumpStatus()
{
#ifdef MKII
	if (digitalRead(PUMP_EN) == HIGH)
	{
		return STOP;
	}
	else
	{
		//Serial.println(getAnalogData(PUMP_FEEDBACK));
		if (getAnalogPinValue(PUMP_FEEDBACK) <= PUMP_GRABBING_CURRENT)
		{
			return GRABBING;
		}
		else
		{
			return WORKING;
		}
	}
#elif defined (METAL)
	if (digitalRead(PUMP_EN) == HIGH)
	{
		return 1;
	}
	else 
	{
		return 0;
	}
#elif defined(SWIFT)
	if (digitalRead(PUMP_EN) == HIGH)
	{
		return STOP;
	}
	else
	{
		double value = getAnalogPinValue(PUMP_FEEDBACK);
		debugPrint("pump value:%d\r\n", value);
		if (value < 10)
		{
			// error, no work
			return STOP;
		}
		else if (value <= PUMP_GRABBING_CURRENT)
		{
			return GRABBING;
		}
		else
		{
			return WORKING;
		}
	}
#endif
}

/*!
   \brief pump working
 */
void pumpOn()
{
#ifdef MKII
	digitalWrite(PUMP_EN, LOW); 
#elif defined(METAL)
	digitalWrite(PUMP_EN, HIGH); 
	digitalWrite(VALVE_EN, LOW);
#elif defined(SWIFT)
	digitalWrite(PUMP_EN, LOW); 
	digitalWrite(VALVE_EN, HIGH);
#endif
}

/*!
   \brief pump stop
 */
void pumpOff()
{


#ifdef MKII
	digitalWrite(PUMP_EN, HIGH); 
#elif defined(METAL)
	digitalWrite(PUMP_EN, LOW); 
	digitalWrite(VALVE_EN, HIGH);
#elif defined(SWIFT)
	digitalWrite(PUMP_EN, HIGH); 
	digitalWrite(VALVE_EN, LOW);
#endif
}


/*!
   \brief get tip status
   \return true if limit switch hit
 */
bool getTip()
{
	if (digitalRead(LIMIT_SW))
	{
		return true;
	}
	else
	{
		return false;
	}
}

/*!
   \brief get pin value
   \param pin of arduino
   \return HIGH or LOW
 */
int getDigitalPinValue(unsigned int pin)
{
	return digitalRead(pin);
}

/*!
   \brief set pin value
   \param pin of arduino
   \param value: HIGH or LOW
 */
void setDigitalPinValue(unsigned int pin, unsigned char value)
{
	if (value)
	{
		digitalWrite(pin, HIGH);
	}
	else
	{
		digitalWrite(pin, LOW);
	}
}

/*!
   \brief get analog value of pin
   \param pin of arduino
   \return value of analog data
 */
int getAnalogPinValue(unsigned int pin)
{
	unsigned int dat[8];


	for(int i = 0; i < 8; i++)
	{
		dat[i] = analogRead(pin);
	}

	_sort(dat, 8);

	unsigned int result = (dat[2]+dat[3]+dat[4]+dat[5])/4;

	return result;    
}

/*!
   \brief convert polor coordinates to Cartesian coordinate
   \param s(mm), r(0~180), h(mm)
   \output x, y, z(mm)
 */
void polToXYZ(double s, double r, double h, double& x, double& y, double& z)
{
	z = h;  
	x = s * cos(r / MATH_TRANS);
	y = s * sin(r / MATH_TRANS);	

	controller.translateCoordiante(x, y, z, true);
}

/*!
   \brief check pos reachable
   \return IN_RANGE if everything is OK
   \return OUT_OF_RANGE_NO_SOLUTION if cannot reach
   \return OUT_OF_RANGE can move to the closest pos
 */
unsigned char validatePos(double x, double y, double z)
{
	double angleRot, angleLeft, angleRight;
	return controller.xyzToAngle(x, y, z, angleRot, angleLeft, angleRight, false);
}

/*!
   \brief get current pos
   \output x, y, z(mm)
 */
void getCurrentXYZ(double& x, double& y, double& z)
{
	controller.updateAllServoAngle();
	controller.getCurrentXYZ(x, y, z);	
}

/*!
   \brief get current pos of polor coordinates
   \output s(mm), r(0~180), h(mm)
 */
void getCurrentPosPol(double& s, double& r, double& h)
{

	double angleRot, angleLeft, angleRight;
	double x, y, z;

	controller.updateAllServoAngle();
	controller.getCurrentXYZ(x, y, z);
	controller.getServoAngles(angleRot, angleLeft, angleRight);
	double stretch;
	stretch = sqrt(x * x + y * y);

	s = stretch;
	r = angleRot;
	h = z;
}

/*!
   \brief get servo angles from pos(x, y, z)
   \param x, y, z(mm)
   \output angles of servo(0~180)
   \return IN_RANGE if everything is OK
   \return OUT_OF_RANGE_NO_SOLUTION if cannot reach
   \return OUT_OF_RANGE can move to the closest pos   
 */
unsigned char xyzToAngle(double x, double y, double z, double& angleRot, double& angleLeft, double& angleRight)
{
	return controller.xyzToAngle(x, y, z, angleRot, angleLeft, angleRight);
}

/*!
   \brief get  pos(x, y, z) from servo angles  
   \param angles of servo(0~180)
   \output x, y, z(mm)
   \return IN_RANGE if everything is OK
   \return OUT_OF_RANGE_NO_SOLUTION if cannot reach
   \return OUT_OF_RANGE can move to the closest pos   
 */
unsigned char angleToXYZ(double angleRot, double angleLeft, double angleRight, double& x, double& y, double& z)
{
	return controller.getXYZFromAngle(x, y, z, angleRot, angleLeft, angleRight);
}

/*!
   \brief get e2prom data
   \param device:  EEPROM_ON_CHIP, EEPROM_EXTERN_USER, EEPROM_EXTERN_SYSTEM
   \param addr: 0~2047(EEPROM_ON_CHIP), 0~65535(EEPROM_EXTERN_USER, EEPROM_EXTERN_SYSTEM)
   \param type: DATA_TYPE_BYTE, DATA_TYPE_INTEGER, DATA_TYPE_FLOAT
 */
double getE2PROMData(unsigned char device, unsigned int addr, unsigned char type)
{
   	double result = 0;

	uint8_t deviceAddr;


	union {
		float fdata;
		uint8_t data[4];
	} FData;


	switch(device)
	{

	case 0:

		switch(type)
		{
			case DATA_TYPE_BYTE:
			{
				int val = EEPROM.read(addr);
				result = val;
				break;
			}
			case DATA_TYPE_INTEGER:
			{
				int i_val = 0;
				EEPROM.get(addr, i_val);
				result = i_val;
				break;
			}
			case DATA_TYPE_FLOAT:
			{
				double f_val = 0.0f;
				EEPROM.get(addr,f_val);
				result = f_val;
				break;
			}
		}

	        break;

	case 1:
		deviceAddr = EXTERNAL_EEPROM_USER_ADDRESS;
        	break;

	case 2:
		deviceAddr = EXTERNAL_EEPROM_SYS_ADDRESS;
        	break;

	default:
		return ADDRESS_ERROR;
	}

    
	if (device == 1 || device == 2)
	{
		int num = 0;
		switch(type)
		{
		case DATA_TYPE_BYTE:
			num = 1;
			break;
		
		case DATA_TYPE_INTEGER:
			num = 2;
			break;
		
		case DATA_TYPE_FLOAT:
			num = 4;
			break;
		
		default:
			return PARAMETER_ERROR;
		}	

		unsigned char i=0;
		i = (addr % 128);
		// Since the eeprom's sector is 128 byte, if we want to write 5 bytes per cycle we need to care about when there's less than 5 bytes left
		if (i >= (129-num)) 
		{
			i = 128 - i;
			iic_readbuf(FData.data, deviceAddr, addr, i);// write data
			delay(5);
			iic_readbuf(FData.data + i, deviceAddr, addr + i, num - i);// write data
		}
		//if the left bytes are greater than 5, just do it
		else
		{
			iic_readbuf(FData.data, deviceAddr, addr, num);// write data
		}      


		switch(type)
		{
		case DATA_TYPE_BYTE:			
			result = FData.data[0];
			break;
			
		case DATA_TYPE_INTEGER:		
			result = (FData.data[0] << 8) + FData.data[1];
			break;
			
		case DATA_TYPE_FLOAT:
			result = FData.fdata;
			break;
			
		}


	}

	return result;

}

/*!
   \brief set e2prom data
   \param device:  EEPROM_ON_CHIP, EEPROM_EXTERN_USER, EEPROM_EXTERN_SYSTEM
   \param addr: 0~2047(EEPROM_ON_CHIP), 0~65535(EEPROM_EXTERN_USER, EEPROM_EXTERN_SYSTEM)
   \param type: DATA_TYPE_BYTE, DATA_TYPE_INTEGER, DATA_TYPE_FLOAT
   \param value: value to write
   \causion: addr < 100 of EEPROM_ON_CHIP is protected 
 */
double setE2PROMData(unsigned char device, unsigned int addr, unsigned char type, double value)
{
	uint8_t deviceAddr;

	union {
		float fdata;
		uint8_t data[4];
	} FData;

	switch(device)
	{

	case 0:    

		// protect area for system parameters
		if (addr < 100)
			return;

        switch(type)
        {
        case DATA_TYPE_BYTE:
        	{
                byte b_val;
                b_val = byte(value);
                EEPROM.write(addr, b_val);
                break;
        	}
        case DATA_TYPE_INTEGER:
        	{
                int i_val = 0;
                i_val = int(value);
                EEPROM.put(addr, i_val);
                break;
        	}
        case DATA_TYPE_FLOAT:
        	{
        	    float f_val = 0.0f;
                f_val = float(value);
                EEPROM.put(addr,f_val);
                // Serial.println(f_val);
                break;
        	}
        }
        break;
	case 1:
		deviceAddr = EXTERNAL_EEPROM_USER_ADDRESS;
		break;

	case 2:
		deviceAddr = EXTERNAL_EEPROM_SYS_ADDRESS;
		break;

	default:
		return ADDRESS_ERROR;
	}       


	if (device == 1 || device == 2)
	{
		int num = 0;
		switch(type)
		{
		case DATA_TYPE_BYTE:
			FData.data[0] = byte(value);
			num = 1;
			break;
			
		case DATA_TYPE_INTEGER:	    
			{
				int i_val = 0;
				i_val = int(value); 
				FData.data[0] = (i_val & 0xff00) >> 8;
				FData.data[1] = i_val & 0xff;
				num = 2;
				break;
			}
		    
		case DATA_TYPE_FLOAT:
			FData.fdata = float(value);
			num = 4;
			break;
		    
		default:
			return PARAMETER_ERROR;
		}

		unsigned char i=0;
		i = (addr % 128);
		// Since the eeprom's sector is 128 byte, if we want to write 5 bytes per cycle we need to care about when there's less than 5 bytes left
		if (i >= (129-num)) 
		{
			i = 128 - i;
			iic_writebuf(FData.data, deviceAddr, addr, i);// write data
			delay(5);
			iic_writebuf(FData.data + i, deviceAddr, addr + i, num - i);// write data
		}
		//if the left bytes are greater than 5, just do it
		else
		{
			iic_writebuf(FData.data, deviceAddr, addr, num);// write data
		}      


       

	}

}

/*!
   \brief test e2prom available
   \param device:  EEPROM_ON_CHIP, EEPROM_EXTERN_USER, EEPROM_EXTERN_SYSTEM
   \return: true if available
 */
bool eeprom_write_test(uint8_t device)
{
	uint16_t value = 0;
	
	if (device > EEPROM_EXTERN_SYSTEM)
	{
		return false;
	}
	
	delay(10);
	setE2PROMData(device, EEPROM_WRITE_TEST_ADDR, DATA_TYPE_INTEGER, 0x5a5a);
	delay(10);
	value = getE2PROMData(device, EEPROM_WRITE_TEST_ADDR, DATA_TYPE_INTEGER);
	if (value != 0x5a5a)
	{
		return false;
	}

	delay(10);
	setE2PROMData(device, EEPROM_WRITE_TEST_ADDR, DATA_TYPE_INTEGER, 0xa5a5);
	delay(10);
	value = getE2PROMData(device, EEPROM_WRITE_TEST_ADDR, DATA_TYPE_INTEGER);
	if (value != 0xa5a5)
	{ 
		return false;
	}	
	
	return true;
}


/*!
   \brief is moving now
 */
bool isMoving()
{
	return (mCurStep != -1);
}

/*!
   \brief stop move immediately
 */
void stopMove()
{
	mCurStep = -1;
}

/*!
   \brief set bluetooth broadcast name
   \return true if success
 */
bool setBtName(char btName[])
{
	int strLength = strlen(btName);

	if (btName == NULL || strLength == 0)
		return false;

	if (strLength > BT_NAME_MAX_LEN)
	{
		btName[BT_NAME_MAX_LEN] = '\0';
	}

	// bluetooth is working
	if (digitalRead(BT_DETECT_PIN) == HIGH)
	{
		return false;
	}

	Serial2.write("AT+NAME");
	Serial2.write(btName);

	delay(100);

	
	// set uuid
	Serial2.write("AT+UUID0xFFE2");

	delay(100);

	unsigned char readBuf[128];
	int i = 0;

	// get data
	while (Serial2.available())
	{
		readBuf[i++] = Serial2.read();
	}

	readBuf[i] = '\0';
	debugPrint("%s\r\n", readBuf);

	// expect data: OK+ADDR:00FF00FF00FF
	// parse datas
	char *s = strstr(readBuf, "OK+Set:");


	return (s != NULL);

}

void ResetBTUUID()
{
	if (digitalRead(BT_DETECT_PIN) == HIGH)
	{
		Serial2.write("AT");
		delay(100);
	}

	// set uuid
	Serial2.write("AT+UUID0xFFE2");	
}

#if defined( MKII	) || defined(SWIFT)

/*!
   \brief check if power plug in
 */
bool isPowerPlugIn()
{
	if (analogRead(POWER_DETECT) > 400)
		return true;
	else
		return false;
}

#endif 

////////////////////////////////////////////////////////////////////////////
// private functions

static void _sort(unsigned int array[], unsigned int len)
{
	unsigned char i=0,j=0;
	unsigned int temp = 0;

	for(i = 0; i < len; i++) 
	{
		for(j = 0; i+j < (len-1); j++) 
		{
			if(array[j] > array[j+1]) 
			{
				temp = array[j];
				array[j] = array[j+1];
				array[j+1] = temp;
			}
		}
	}	
}

static void _interpolate(double startVal, double endVal, double *interpVals, int steps, byte easeType)
{

	startVal = startVal / 10.0;
	endVal = endVal / 10.0;

	double delta = endVal - startVal;
	for (byte i = 1; i <= steps; i++)
	{
		float t = (float)i / steps;
		//*(interp_vals+f) = 10.0*(start_val + (3 * delta) * (t * t) + (-2 * delta) * (t * t * t));
		*(interpVals + i - 1) = 10.0 * (startVal + t * t * delta * (3 + (-2) * t));
	}
}

static unsigned char _moveTo(double x, double y, double z, double speed)
{
	
	double angleRot = 0, angleLeft = 0, angleRight = 0;
	double curRot = 0, curLeft = 0, curRight = 0;
	double targetRot = 0;
	double targetLeft = 0;
	double targetRight = 0;
	double curX = 0;
	double curY = 0;
	double curZ = 0;
	int i = 0;
	int totalSteps = 0;
	unsigned int timePerStep;

	unsigned char status = 0;

	status = controller.xyzToAngle(x, y, z, targetRot, targetLeft, targetRight);
	debugPrint("target B=%f, L=%f, R=%f\r\n", targetRot, targetLeft, targetRight);

	if (status == OUT_OF_RANGE_NO_SOLUTION)
	{
		return OUT_OF_RANGE_NO_SOLUTION;
	}

	if (speed == 0)
	{
		mCurStep = -1;
		controller.writeServoAngle(targetRot, targetLeft, targetRight);
		return IN_RANGE;
	}

	// get current angles
	controller.getServoAngles(curRot, curLeft, curRight);
	// get current xyz
	controller.getCurrentXYZ(curX, curY, curZ);

	debugPrint("cur B=%f, L=%f, R=%f\r\n", curRot, curLeft, curRight);

	// calculate max steps
	totalSteps = max(abs(targetRot - curRot), abs(targetLeft - curLeft));
	totalSteps = max(totalSteps, abs(targetRight - curRight));

	if (totalSteps <= 0)
		return NO_NEED_TO_MOVE;

	totalSteps = totalSteps < STEP_MAX ? totalSteps : STEP_MAX;

	// calculate step time
	double distance = sqrt((x-curX) * (x-curX) + (y-curY) * (y-curY) + (z-curZ) * (z-curZ));
	speed = constrain(speed, 100.00, 60000.00);


	
	
	timePerStep = distance * 1000.0 * 60 / totalSteps / speed ;


/*
	// keep timePerStep <= STEP_MAX_TIME
	if (timePerStep > STEP_MAX_TIME)
	{
		double ratio = double(timePerStep) / STEP_MAX_TIME;

		if (totalSteps * ratio < STEP_MAX)
		{
			totalSteps *= ratio;
			timePerStep = STEP_MAX_TIME;
		}
		else
		{
			totalSteps = STEP_MAX;
			timePerStep = STEP_MAX_TIME;
		}
	}
*/


	totalSteps = totalSteps < STEP_MAX ? totalSteps : STEP_MAX;



	// trajectory planning
	_interpolate(curX, x, mPathX, totalSteps, INTERP_EASE_INOUT_CUBIC);
	_interpolate(curY, y, mPathY, totalSteps, INTERP_EASE_INOUT_CUBIC);
	_interpolate(curZ, z, mPathZ, totalSteps, INTERP_EASE_INOUT_CUBIC);

	for (i = 0; i < totalSteps; i++)
	{
		status = controller.xyzToAngle(mPathX[i], mPathY[i], mPathZ[i], angleRot, angleLeft, angleRight);

		if (status != IN_RANGE)
		{
			break;
		}
		else
		{
			mPathX[i] = angleRot;
			mPathY[i] = angleLeft;
			mPathZ[i] = angleRight;
		}
	}

	if (i < totalSteps)
	{
		//debugPrint("i < totalSteps\r\n");
		_interpolate(curRot, targetRot, mPathX, totalSteps, INTERP_EASE_INOUT_CUBIC);
		_interpolate(curLeft, targetLeft, mPathY, totalSteps, INTERP_EASE_INOUT_CUBIC);
		_interpolate(curRight, targetRight, mPathZ, totalSteps, INTERP_EASE_INOUT_CUBIC);    	
	}

	mPathX[totalSteps - 1] = targetRot;
	mPathY[totalSteps - 1] = targetLeft;
	mPathZ[totalSteps - 1] = targetRight;


	mTimePerStep = timePerStep;
	mTotalSteps = totalSteps;
	mCurStep = 0;
	mStartTime = millis();

	return IN_RANGE;
}

static unsigned char _moveToAngle(uint8_t servoNumber, double angle, double speed)
{
	// validate destination angle

	// get cur angle 
	double curAngle = getServoAngle(servoNumber);
	debugPrint("curAngle = %f\r\n", curAngle);

	double deltaAngle = angle - curAngle ;

	// calculate steps
	int totalSteps = abs(curAngle - angle);


	if (totalSteps <= 0)
		return NO_NEED_TO_MOVE;

	speed = constrain(speed, 100, 60000);
	totalSteps = totalSteps < STEP_MAX ? totalSteps : STEP_MAX;	
	

	unsigned int timePerStep = abs(deltaAngle) / speed * 1000 * 60 / totalSteps;
	

	for (int i = 0; i < totalSteps; i++)
	{
		mPathX[i] = curAngle + (deltaAngle / totalSteps) * i;
		debugPrint("mPathX[i] = %f\r\n", mPathX[i]);
	}

	mPathX[totalSteps - 1] = angle; 


	mTimePerStep = timePerStep;
	mTotalSteps = totalSteps;
	mCurStep = 0;
	mCurIndex = servoNumber;
	mStartTime = millis();

	debugPrint("mCurIndex = %d\r\n", mCurIndex);
	return IN_RANGE;



}

static void _controllerAngleRun()
{
	while (mCurStep >= 0 && mCurStep < mTotalSteps)
	{

		if((millis() - mStartTime) >= (mCurStep * mTimePerStep)) 
		{

			controller.writeServoAngle(mCurIndex, mPathX[mCurStep]);


              
			mCurStep++;


			if (mCurStep >= mTotalSteps)       
			{
				mCurStep = -1;
			}  
		}	

		manage_inactivity();
	}
}

static void _controllerRun()
{


	while (mCurStep >= 0 && mCurStep < mTotalSteps)
	{

		if((millis() - mStartTime) >= (mCurStep * mTimePerStep)) 
		{
			
            // ignore the point if cannot reach
			if (controller.limitRange(mPathX[mCurStep], mPathY[mCurStep], mPathZ[mCurStep]) != OUT_OF_RANGE_NO_SOLUTION)
			{
				//debugPrint("curStep:%d, %f, %f, %f\r\n", mCurStep, mPathX[mCurStep], mPathY[mCurStep], mPathZ[mCurStep]);
				
				controller.writeServoAngle(mPathX[mCurStep], mPathY[mCurStep], mPathZ[mCurStep]);
			}
              
			mCurStep++;


			if (mCurStep >= mTotalSteps)       
			{
				mCurStep = -1;
			}  
			
			
		}	

		manage_inactivity();
		
		
	}

}